#include "motion.h"

steeping_motor motor1, motor2;
static char now_location = 0;

//���������ʼ��
void motion_Init()
{
    motor1.motor_ID = 1;
	motor2.motor_ID = 2;

	//usart_ctrl_gimbal_motor(motor2);
}

//��̨����
void gimbal_motor_up(char choose_place)
{
    motor2.motor_acc_speed = 230;
	motor2.motor_direction = 0;
	motor2.motor_pulse_num = 12500;
	motor2.synchronous = 0;
	motor2.motor_speed = 300;

	if(choose_place == ground) 						motor2.motor_pulse_num = ground_high;
	else if(choose_place == plate) 					motor2.motor_pulse_num = plate_high;
	else if(choose_place == second_floor)	motor2.motor_pulse_num = second_floor_high;
	else if(choose_place == turn_table) 	motor2.motor_pulse_num = turn_table_high;

	usart_ctrl_gimbal_motor(motor2);
}

//��̨�½�
void gimbal_motor_down(char choose_place)
{
    motor2.motor_acc_speed = 230;
	motor2.motor_direction = 1;
	motor2.synchronous = 0;          
	motor2.motor_speed = 300;

	if(choose_place == ground) 					motor2.motor_pulse_num = ground_high;
	else if(choose_place == plate) 				motor2.motor_pulse_num = plate_high;
	else if(choose_place == second_floor)	 	motor2.motor_pulse_num = second_floor_high;
	else if(turn_table) 						motor2.motor_pulse_num = turn_table_high;
	
	usart_ctrl_gimbal_motor(motor2);
}

//��̨ת��
//ȡ�м�����ʱ������������������Զഫ��һ������ָʾȡ�м�ʱ��ת��
void gimbal_motor_turn(char choose_disc, char plate_2_way)
{
	static char last_direction = 0;
	static int last_pulse_num = 0;

	if(choose_disc == 1) 
	{
		motor1.motor_direction = 0;
		motor1.motor_pulse_num = 1200;
	}
	else if(choose_disc == 2)
	{
		motor1.motor_direction = plate_2_way;
		motor1.motor_pulse_num = 1610;
	}
	else if(choose_disc == 3)
	{
		motor1.motor_direction = 1;
		motor1.motor_pulse_num = 1160;
	}
	else if(choose_disc == return_to_front)
	{
		motor1.motor_direction = !last_direction;
		motor1.motor_pulse_num = last_pulse_num;
		//vTaskDelay(1);//BUG��ʱ1ms����������ִ��ִ�У�ԭ����
	}

    motor1.motor_acc_speed = 80;
	motor1.synchronous = 0;
	motor1.motor_speed = 100;

	last_direction = motor1.motor_direction;
	last_pulse_num = motor1.motor_pulse_num;

	usart_ctrl_gimbal_motor(motor1);
}

// �ӵ��ϼ�������
void get_goods_on_ground()
{
	set_servo_degree(open);
	gimbal_motor_down(ground);
	delay_xms(700);
	set_servo_degree(close);
	delay_xms(500);
	gimbal_motor_up(ground);
	delay_xms(600);
}

//�������ϵ�����
void put_goods_on_plate(char choose_disc)
{
	gimbal_motor_turn(choose_disc, plate_2_turn_left);
	delay_xms(800);
	gimbal_motor_down(plate);
	delay_xms(500);
	set_servo_degree(open_small);
	delay_xms(400);
	gimbal_motor_up(plate);
	delay_xms(500);
	gimbal_motor_turn(return_to_front, plate_2_NULL_turn);
	set_servo_degree(open);

	//vTaskDelay(1500);//����ȥ��
}

//�����Ӽ�������
void get_goods_on_plate(char choose_disc)
{
	gimbal_motor_turn(choose_disc, plate_2_turn_right);
	delay_xms(800);
	set_servo_degree(open_small);
	gimbal_motor_down(plate);
	delay_xms(600);
	set_servo_degree(close);
	gimbal_motor_up(plate);
	delay_xms(500);
	gimbal_motor_turn(return_to_front, plate_2_NULL_turn);
	delay_xms(1000);
}

//�����Ϸ��ڵ���
void put_goods_on_ground()
{
	//�����Ϸŵ�����
	gimbal_motor_down(ground);
	delay_xms(1100);
	set_servo_degree(open_small);
	gimbal_motor_up(ground);
	delay_xms(200);
	set_servo_degree(open);

	delay_xms(1200);//����ȥ��
}

//������
void put_goods_on_second_floor()
{
	delay_xms(200);//ȥ��
	gimbal_motor_down(second_floor);
	delay_xms(800);
	set_servo_degree(open_small);
	gimbal_motor_up(second_floor);
	delay_xms(200);
	set_servo_degree(open);

	delay_xms(1200);//����ȥ��
}

//��ת�̼�ȡ����
void get_goods_on_turn_table()
{
	gimbal_motor_down(turn_table);
	delay_xms(400);
	set_servo_degree(close);
	delay_xms(400);
	gimbal_motor_up(turn_table);
}

void suspend_motion_task()
{
	vTaskSuspend(speed_x_handler);
	vTaskSuspend(speed_y_handler);
	vTaskSuspend(speed_w_handler);
	vTaskSuspend(speed_mix_handler);
}

void resume_motion_task()
{
	vTaskResume(speed_x_handler);
	vTaskResume(speed_y_handler);
	vTaskResume(speed_w_handler);
	vTaskResume(speed_mix_handler);
}

//��ȡ������Ʒ�ķ�����뺯��
int* get_way(char location)
{
	static int out_location[2], count = 0;

	if (now_location == 0)
	{
		out_location[0] = out_location[1] = 0;
		now_location = location;
	}
	else if (location > now_location)
	{
		out_location[0] = (location - now_location) * 700;	//time
		out_location[1] = 50;								//speed
	}
	else if (location < now_location)
	{
		out_location[0] = (now_location -location) * 700;	//time
		out_location[1] = -50;								//speed
	}
	now_location = location;

	count++;
	if (count == 6) now_location = count = 0;	//ÿ���θ�λ
	return out_location;
}

//Բ�Ķ�λ
int* circle_location()
{
	static int* location_data = NULL;
	static int err[2];
	location_data = circle_location_decoding();

	if(location_data != NULL)
	{
		err[0] = (center_x_location - location_data[0]);
		err[1] = (center_y_location - location_data[1]);
	
		//printf("x:%d y:%d errx:%d erry:%d\r\n", location_data[0], location_data[1], err[0], err[1]);

		return err;
	}
	else return NULL;
}

//�߶�λ
int* line_location()
{
	static int* line_location_data = NULL;
	line_location_data = line_location_decoding();
	
	if(line_location_data != NULL) 
	{
		//printf("%d %d\r\n", line_location_data[0], line_location_data[1]);
		return line_location_data;
	}
	else return NULL;
	
}

void circle_location_task(char p_color)
{
	int* location_x_y_data = NULL;

	reset_circle_location_packge();

	while(1)
	{
		call_pi(circle_center_identify + p_color);
		delay_xms(100);

		location_x_y_data = NULL;
		location_x_y_data = circle_location();	// ���㶨λx��y

		if(	location_x_y_data != NULL)//���յ�����
		{
			//�������ж϶�λ���
			if(	((location_x_y_data[0] > -circle_dead_area) && (location_x_y_data[0] < circle_dead_area)) &&
				((location_x_y_data[1] > -circle_dead_area) && (location_x_y_data[1] < circle_dead_area)))
			{
				printf("finish\r\n");
				change_xyw_speed_no_quene(0, 0, 0);
				return;
			}

			//�޷�������
			//TODO:�ֶ��ٶȶ�λ
			for(char i = 0; i < 2; i++)
			{
				if(location_x_y_data[i] < -circle_second_area) location_x_y_data[i] = -3;
				else if(location_x_y_data[i] > circle_second_area) location_x_y_data[i] = 3;
				else if((location_x_y_data[i] > -circle_second_area) && (location_x_y_data[i] < -1)) location_x_y_data[i] = -1;
				else if((location_x_y_data[i] < circle_second_area) && (location_x_y_data[i]) > 1) location_x_y_data[i] = 1;
			}

			change_xyw_speed_no_quene(location_x_y_data[0], location_x_y_data[1], 0);
		}
	}
}

void line_location_task()
{
	static int* line_data;

	reset_line_location_packge();

	while(1)
	{
		call_pi(line_degree_location);
		delay_xms(50);
		line_data = line_location();
		line_data[0] = line_data[0] + line_correct_data;
		if(line_data != NULL)
		{
			if(	(line_data[0] > -line_degree_dead_area && line_data[0] < line_degree_dead_area) )
			{
				printf("finish!\r\n");
				change_xyw_speed_no_quene(0, 0, 0);
				return;
			}
			
			if(line_data[0] < -2) line_data[0] = -3;
			else if(line_data[0] > 2) line_data[0] = 3;
			else if(line_data[0] > 0 && line_data[0] < 2) line_data[0] = 1;
			else if(line_data[0] < 0 && line_data[0] < -2) line_data[0] = 1;
			change_xyw_speed_no_quene(0, 0, line_data[0]);

		}
		
	}

}
